/* 
 * A664p7_vl.c -- Implements a basic VL layer on top of the A664P7 subsystem.
 *
 *
 *------------------------------------------------------------------------------
 *
 *                    ---   PROPRIETARY NOTICE   ---
 *
 *   The information contained herein is proprietary to Smiths Aerospace.
 *   and shall not be reproduced or disclosed in whole or in part or used
 *   for any design or manufacture except when such user possesses direct
 *   written authorization from Smiths Aerospace.
 *
 *   Copyright 2006, Smiths Aerospace.
 *
 *------------------------------------------------------------------------------
 *
 * modification history
 * --------------------
 * 30 mar 06	Huizenga	Initial revision.
 * 10 may 06	Huizenga	Added additional comments
 * 10 may 06	Huizenga	Updated functions to use the standardized success/fail constants
 * 02 jun 06	Huizenga	Fixed size of rm seq. num (16 bit -> 8 bit)
 * 05 jun 06	Huizenga	Fixed create_vl to generate a minimum packet size of 60 bytes
 *							(smaller messages must be padded to 60)
 * 05 jun 06	Huizening	Fixed read_vl_message to only copy the UDP payload (instead of payload & pad)
 */

/*******************************************************************************************
 * NOTICE: The VL layer of the A664P7 system is NOT reentrant.  It is assumed that any given *
 *			routine from this layer will complete execution before any other routine from  *
 *			this layer is called. As a result, this layer of the library should only be    *
 *			accessed by a single thread at any given time! If you absolutely must access   *
 *			this layer from more than one thread at once, your application must manage its *
 *			own accesses to ensure that only one routine from this layer is active at any  *
 *			given time.                                                                    *
 *******************************************************************************************/

#include "pcap.h"

#ifndef WIN32
	#include <sys/socket.h>
	#include <netinet/in.h>
#else
	#include <winsock.h>
#endif
#include "A664p7_internal.h"

/* Global list of all created VLs -- This is used while sorting incoming messages into VLs*/
VL_LIST *vl_lst;

/* Global list of all devices to receive on -- This is used while sorting messages into VLs*/
VL_DEVICE_LIST *rx_vl_devices;

/* 
 * push_vl_device:
 *   Adds WinPCap ethernet device to a VL_DEVICE_LIST.
 * INPUTS:  list_top -- A pointer to the item most recently added to the current list (the "top" of the stack)
 *			dev      -- A pointer to the device to add to the list
 * RETURNS: A pointer to the newly added list item (the new "top" of the stack)
 */
VL_DEVICE_LIST *push_vl_device(VL_DEVICE_LIST *list_top, pcap_t *dev)
{
	VL_DEVICE_LIST *new_node = NULL;
	VL_DEVICE_LIST *iterator = list_top;

/* If device is already in the list, don't re-add it */
	while(iterator)
	{
		/* If we find it, just return the old list */
		if(iterator->dev == dev)
			return list_top;

		iterator = iterator->next_dev;
	}

/* If the machine can't spare the RAM, fail (out-of-memory)*/
	if(!(new_node = calloc(sizeof(VL_DEVICE_LIST), 1)))
		return NULL;

	new_node->next_dev = list_top;
	new_node->dev = dev;

	return new_node;
}

/* 
 * push_vl:
 *   Adds VL to a VL_LIST.
 * INPUTS:  list_top -- A pointer to the item most recently added to the current list (the "top" of the stack)
 *			vl       -- A pointer to the device to add to the list
 * RETURNS: A pointer to the newly added list item (the new "top" of the stack)
 */
VL_LIST *push_vl(VL_LIST *list_top, VL *vl)
{
	VL_LIST *new_node = NULL;
	VL_LIST *iterator = list_top;

/* If device is VL in the list, don't re-add it */
	while(iterator)
	{
		/* If we find it, just return the old list */
		if(iterator->vl == vl)
		{
			printf("Warning: Attempted to add the same VL to the global list twice!\n");
			return list_top;
		}

		iterator = iterator->next_vl;
	}

	if(!(new_node = calloc(sizeof(VL_LIST), 1)))
		return NULL;

	new_node->next_vl = list_top;
	new_node->vl = vl;

	return new_node;
}


/* VL_LISTs and VL_DEVICE_LISTs are constant throughout a program's execution, so there
	are no functions to destroy them.  The memory will be freed by the operating system
	when the program exits.  If you desire to implement a program that allows the user
	to destroy VLs or inactivate devices on the fly, you would have to implement a function
	capable of properly removing elements from these lists in order to avoid a memory leak.
*/


/* 
 * destroy_vl:
 *   Frees a VL structure and its associated PACKET.
 * INPUTS:  vl -- A pointer to the VL to destroy
 */
void destroy_vl(VL *vl)
{
	if(vl)
	{
		if(vl->pkt)
			destroy_packet(vl->pkt);

		free(vl);
	}
}

/* 
 * populate_vl:
 *   Populates the members of a VL structure
 * INPUTS:  vl      -- A pointer to the VL to populate
 *          pkt     -- A pointer to the packet that is to be associated with "vl"
 *          vl_num  -- The VL_ID to be assigned to the VL
 *          vl_mode -- A value indicating the transmit/recieve mode for the vl
 *          na_dev  -- A pointer to the winpcap device to use for "Network A"
 *          nb_dev  -- A pointer to the winpcap device to use for "Network B"
 * RETURNS: A664P7_FAIL on failure, A664P7_SUCC on success
 */
int populate_vl(VL *vl, PACKET *pkt, int vl_num, int vl_mode, pcap_t *na_dev, pcap_t *nb_dev)
{
	if(!vl)
		return A664P7_FAIL;

	vl->pkt = pkt;
	vl->vl_num = vl_num;
	vl->vl_mode = vl_mode;
	vl->net_a_dev = na_dev;
	vl->net_b_dev = nb_dev;

	return A664P7_SUCC;
}

/* 
 * create_vl:
 *   Creates a fully populated VL and the associated PACKET. Destination MAC addresses are
 *   computed automatically.  The new VL is automatically added to the list of VLs managed by
 *   the tx/rx system.
 * INPUTS:  vl_num  -- VL_ID of the created VL
 *          vl_mode -- A value indicating the transmit/recieve mode for the vl
 *          na_dev  -- A pointer to the winpcap device to use for "Network A"
 *          nb_dev  -- A pointer to the winpcap device to use for "Network B"
 *          max_message_size  -- Maximum message size to be handled by the VL (fragmentation is not supported)
 *          src_mac -- A buffer containing the source mac address to be associated with the VL
 *          src_ip  -- An unsigned integer containing the source IP address to be associated with the VL
 *          dst_ip  -- An unsigned integer containing the destination IP address to be associated with the VL
 *          src_port-- An unsigned short integer containing the source port to be associated with the VL
 *          dst_port-- An unsigned short integer containing the destination port to be associated with the VL
 * RETURNS: A pointer to the newly created VL on success, NULL on failure
 */
VL *create_vl(int vl_num, int vl_mode, pcap_t *na_dev, pcap_t *nb_dev, int max_message_size, 
				 unsigned char src_mac[6],
				 unsigned int src_ip, unsigned int dst_ip,
				 unsigned short int src_port, unsigned short int dst_port)
{
	VL *vl;
	PACKET *pkt;
	char *vlid = (char *)&vl_num;
	char dst_mac[6];
	dst_mac[0] = 0x03;
	dst_mac[1] = 0x00;
	dst_mac[2] = 0x00;
	dst_mac[3] = 0x00;
	dst_mac[4] = 0x00;
	dst_mac[5] = 0x01;

	/* We need a buffer big enough to bring have a packet of 60 bytes min., even if the user doesn't need it */
	if(max_message_size < MIN_A664P7_MESSAGE_SIZE)
		max_message_size = MIN_A664P7_MESSAGE_SIZE;

	if(!(vl = calloc(sizeof(VL),1)))
	{
		printf("Could not create VL with VLID %i!\n", vl_num);
		return NULL;
	}

	if(!(pkt = create_packet(max_message_size)))
	{
		destroy_vl(vl);
		printf("Could not create packet for VL with VLID %i!\n", vl_num);
		return NULL;
	}

	/* Set VLID */
	dst_mac[5] = vlid[0];
	dst_mac[4] = vlid[1];

	populate_ethernet_header(pkt->eh, src_mac, dst_mac, htons(ETH_TYPE_IP));
	populate_ip_header(pkt->ih, src_ip, dst_ip);
	populate_udp_header(pkt->uh, src_port, dst_port);
	populate_vl(vl, pkt, vl_num, vl_mode, na_dev, nb_dev);

	/* Add the VL to the global list */
	if(!(vl_lst = push_vl(vl_lst, vl)))
	{
		printf("Fatal error: Couldn't add new VL.  Exiting.\n");
		exit(EXIT_FAILURE);
	}

	/* Add the devices to the global list */
	if(na_dev && (vl->vl_mode & VL_RX_A))
	{
		if(!(rx_vl_devices = push_vl_device(rx_vl_devices, na_dev)))
		{
			printf("Fatal error: Couldn't add new VL's Net A device to the rx devices list!\n");
			exit(EXIT_FAILURE);
		}
	}

	if(nb_dev && (vl->vl_mode & VL_RX_B))
	{
		if(!(rx_vl_devices = push_vl_device(rx_vl_devices, nb_dev)))
		{
			printf("Fatal error: Couldn't add new VL's Net B device to the rx devices list!\n");
			exit(EXIT_FAILURE);
		}
	}
	
	return vl;
}

/* 
 * rx_vl_packet_match:
 *   Compares the identifying parameters of the data in PACKET "p" with those of VL "vl"
 *  INPUTS:  p      -- PACKET to check
 *           device -- A pointer to the device on which PACKET "p" was recieved
 *           vl     -- VL to check against
 *  RETURNS: A bitfield containing flags indicating which parameters did not match.
 *           A return value of VL_PKT_MATCH (0) indicates a full match.
 */
int rx_vl_packet_match(PACKET *p, pcap_t *device, VL *vl)
{
	int vl_flags = VL_PKT_MATCH;
/*	currently unused
	int ip_check_temp; */

	/* The "correct" parameters for a given rx VL are those that are already in its
		packet.  These are pre-set for rx vl's when they are created, and by 
		verifying that they match on incoming packets, we know that they'll stay
		that way, even when we overwrite old rx vl packets with incoming ones */
	
	/* If it didn't come in on a device assigned to the VL */
	if( 
		!( 
			((device == vl->net_a_dev) && (vl->vl_mode &  VL_RX_A)) || 
			((device == vl->net_b_dev) && (vl->vl_mode &  VL_RX_B)) 
		)
	  )
		vl_flags |= VL_PHYS_INTERFACE;

	/* If it's longer than the max msg length */
	if( p->data_length  > vl->pkt->data_buffer_length )
		vl_flags |= VL_LENGTH;

	/* If the dest MAC doesn't match (VLID) */
	if( memcmp(p->eh->dest, vl->pkt->eh->dest, 6) )
		vl_flags |= VL_DEST_MAC;

	/* If the checksum is wrong */
/*	if( (ip_check_temp = generate_ip_checksum(p->ih)) != p->ih->check )
		vl_flags |= VL_IP_CHECK;
*/
	/* Currently, we can't receive non-UDP packets, so this wont happen */
	if( p->ih->protocol != PROT_UDP)
		vl_flags |= VL_IP_PROTO;

	/* finish checking that new_packet == old_packet (Yadda yadda) .... */

	if( p->ih->ver != 4)
		vl_flags |= VL_IP_VER;

	if( p->ih->hl != 5)
		vl_flags |= VL_IP_LEN;

	if( p->ih->dst != vl->pkt->ih->dst)
		vl_flags |= VL_IP_ADDR;

	if( p->uh->d_port != vl->pkt->uh->d_port)
		vl_flags |= VL_UDP_PORT;
	return vl_flags;
}
		
void print_vl_flags(int vl_flags)
{
	if(vl_flags & VL_PHYS_INTERFACE)
		printf("VL_PHYS_INTERFACE ");

	if(vl_flags & VL_LENGTH)
		printf("VL_LENGTH ");

	if(vl_flags & VL_DEST_MAC)
		printf("VL_DEST_MAC ");

	if(vl_flags & VL_IP_CHECK)
		printf("VL_IP_CHECK ");

	if(vl_flags & VL_IP_PROTO)
		printf("VL_IP_PROTO ");

	if(vl_flags & VL_IP_VER)
		printf("VL_IP_VER ");

	if(vl_flags & VL_IP_LEN)
		printf("VL_IP_LEN ");

	if(vl_flags & VL_IP_ADDR)
		printf("VL_IP_ADDR ");

	if(vl_flags & VL_UDP_PORT)
		printf("VL_UDP_PORT ");
}

/* 
 * count_flag_bits:
 *   Counts the number of "1" bits in an unsigned integer
 *  INPUTS:  bitfield -- The bitfield to be counted
 *  RETURNS: Number of "1"s in "bitfield"
 */
int count_flag_bits(unsigned int bitfield)
{
	int count = 0;
	for(; bitfield > 0; bitfield >>= 1)
		if(bitfield & 1)
			count++;
	return count;
}

/* 
 * distribute_packet_to_vls:
 *   Attempts to distribute PACKET "p" to VLs in VL_LIST "vls".  "p" will be
 *   distributed to every VL that has matching parameters in "vls".  If the
 *   packet is rejected, the value pointed to by *reject_flags can be examined
 *   to determine why.
 *  INPUTS:  p      -- A pointer to the packet to distribute
 *           device -- A pointer to the device that "p" was recieved on
 *			 vls	-- The LV_LIST to attempt to distribute "p" to
 *  RETURNS: Number of "1"s in "bitfield"
 *			 *reject_flags -- The value pointed to by *reject_flags will be set to
 *                            VL_PKT_MATCH if the "p" was distributed to any VLs.
 *                            Otherwise, the value will be set to indicate that
 *                            non-matching fields of the best matching VL.
 */
int distribute_packet_to_vls(PACKET *p, pcap_t *device, VL_LIST *vls, int *reject_flags)
{
	VL_LIST *iterator = vls;
	int distribution_count = 0;
	unsigned int best_flag_set = 0xFFFFFFFF;
	unsigned int current_flag_set;
	while(iterator)
	{
		current_flag_set = rx_vl_packet_match(p, device, iterator->vl);

		/* If we got a better match this time, record it */
		if(count_flag_bits(current_flag_set) < count_flag_bits(best_flag_set))
			best_flag_set = current_flag_set;

		/* If we got a match, copy it */
		if(current_flag_set == VL_PKT_MATCH)
		{
			copy_packet(iterator->vl->pkt, p);
			iterator->vl->new_message |= VL_RX_NEW;
			distribution_count++;
		}
			
		iterator = iterator->next_vl;
	}

	*reject_flags = best_flag_set;
	return distribution_count;
}

/* 
 * rx_all_vls:
 *  Reads through all messages in the recieve frame buffer and distributes them
 *  to VLs
 *  RETURNS: Number of Message->VL distributions to take place (includes all
 *           distributions, even those that have been overwritten by a newer
 *           frame read from the recieve frame buffer).
 */
int rx_all_vls()
{
	PACKET *p;
	int flags_result = 0;

	VL_DEVICE_LIST *current_dev = rx_vl_devices;
	int vls_updated = 0;
	
	/* iterate throught the rx device list */
	while(current_dev)
	{
		/* read & distribute the packets from each device */
		while(p = read_A664p7_packet(current_dev->dev))
		{
			vls_updated += distribute_packet_to_vls(p, current_dev->dev, vl_lst, &flags_result);

			/* If incoming messages aren't being matched to a VL (and you expect them to be), uncomment
			   this as a first step in your investigation.  Please note that due to winpcap's architecture,
			   all outgoing messages are also recieved, and therefore will show up as messages that could
			   not be matched to a VL (unless you have your VLs set up to do loopback)
			
			if(flags_result != VL_PKT_MATCH)
			{
				printf("Couldn't assign an incoming message because the following parameters could not be met by an assigned VL:\n");
				print_vl_flags(flags_result);
				printf("\n");
			} */
		}
		current_dev = current_dev->next_dev;
	}

	return vls_updated;
}

/* 
 * tx_vl: Transmits the packet assigned to "vl"
 * INPUTS: vl -- vl to transmit
 * RETURNS: Interfaces VL was transmitted on, using vl_mode flags
 */
int tx_vl(VL *vl)
{
	int size;
	int ret = 0;
	setup_packet_pointers(vl->pkt);
	if(!IS_SUCCESS(convert_packet_header_byte_order(vl->pkt, PACKET_ORDER_NET)))
		return 0;

	size = sizeof(ETHERNET_HEADER) + 
				sizeof(IP_HEADER) + 
				sizeof(UDP_HEADER) + 
				vl->pkt->data_length + RM_SEQ_NUM_LEN; 		// size

	*(vl->pkt->rm_seq_num) = vl->rm_seq_num;
	vl->rm_seq_num++;

	if((vl->vl_mode & VL_TX_A) && vl->net_a_dev)
	{
		vl->pkt->eh->source[5] &= 0x1F;	/* Clear the interface ID */
		vl->pkt->eh->source[5] |= 0x20; /* Set interface A */
		if (IS_SUCCESS(send_A664p7_packet(vl->pkt, vl->net_a_dev)))
		{
			ret |= VL_TX_A;
		}
		else
		{
			printf("\nError sending the packet for VL %i on interface A! (%s)\n", vl->vl_num, pcap_geterr(vl->net_a_dev));
		}
	}

	if((vl->vl_mode & VL_TX_B) && vl->net_b_dev)
	{
		vl->pkt->eh->source[5] &= 0x1F;	/* Clear the interface ID */
		vl->pkt->eh->source[5] |= 0x40; /* Set interface B */
		if (IS_SUCCESS(send_A664p7_packet(vl->pkt, vl->net_b_dev)))
		{
			ret |= VL_TX_B;
		}
		else
		{
			printf("\nError sending the packet for VL %i on interface B! (%s)\n", vl->vl_num, pcap_geterr(vl->net_b_dev));
		}
	}

	vl->new_message &= ~VL_TX_NEW;
	return ret;
}

/* 
 * tx_updated_vls: Transmits any VLs that have messages with messages that have been newly written but
 *                 not yet transmitted.
 */
void tx_updated_vls()
{
	VL_LIST *iterator = vl_lst;
	while(iterator)
	{
		if(iterator->vl->new_message & VL_TX_NEW)
		{
			tx_vl(iterator->vl);
		}
		iterator = iterator->next_vl;
	}
}

/* 
 * tx_all_vls: Transmits all txVLs
 */
void tx_all_vls()
{
	VL_LIST *iterator = vl_lst;
	while(iterator)
	{
		tx_vl(iterator->vl);
		iterator = iterator->next_vl;
	}
}

/* 
 * read_vl_message: Copies the payload from the message assigned to "vl" to "buffer"
 * INPUTS: vl -- vl to read
 *         buffer -- Buffer to which payload will be copied
 *         buffer_size -- size of "buffer" in bytes
 * RETURNS: Size of copied data, in bytes (0 if no data was copied)
 */
int read_vl_message(VL *vl, char *buffer, int buffer_size)
{
	int udp_payload_size = 0;
	/* Warning: these MUST be checked IN THIS ORDER! Do not change..*/
	if((!vl) || (!buffer) || (!vl->pkt) || (!vl->pkt->data))
		return 0;

	convert_packet_header_byte_order(vl->pkt, PACKET_ORDER_HOST);

	udp_payload_size = vl->pkt->uh->length - sizeof(UDP_HEADER);

	if(buffer_size < udp_payload_size)
		return 0;

	memcpy(buffer, vl->pkt->data, vl->pkt->uh->length - sizeof(UDP_HEADER));

	/* Turn off the new RX message flag */
	vl->new_message &= ~VL_RX_NEW;

	return udp_payload_size;
}

/* 
 * write_vl_message: Copies the payload from the "buffer" to the message assigned to "vl"
 * INPUTS: vl -- vl to read
 *         buffer -- Buffer from which payload will be copied
 *         message_size -- size of the payload in bytes
 * RETURNS: 0 on failure, number of bytes written on success
 */
int write_vl_message(VL *vl, char *buffer, int message_size)
{
	int tx_message_size = message_size;

	/* Warning: these MUST be checked IN THIS ORDER! Do not change..*/
	if((!vl) || (!buffer) || (!vl->pkt) || (!vl->pkt->data))
		return 0;

	convert_packet_header_byte_order(vl->pkt, PACKET_ORDER_HOST);

	if(vl->pkt->data_buffer_length < message_size)
	{
		convert_packet_header_byte_order(vl->pkt, PACKET_ORDER_NET);
		return 0;
	}

	/* Make sure the packet as a whole is at least 60 bytes long */
	if ( tx_message_size < MIN_A664P7_MESSAGE_SIZE )
	{
		tx_message_size = MIN_A664P7_MESSAGE_SIZE;
		memset(vl->pkt->data + message_size, 0, tx_message_size - message_size);
	}

	/* copy the sucker */
	memcpy(vl->pkt->data, buffer, message_size);

	vl->pkt->ih->len = message_size + sizeof(UDP_HEADER) + sizeof(IP_HEADER);
	vl->pkt->uh->length = message_size + sizeof(UDP_HEADER);
	vl->pkt->data_length = tx_message_size;
	setup_packet_pointers(vl->pkt);
	convert_packet_header_byte_order(vl->pkt, PACKET_ORDER_NET);
	vl->new_message |= VL_TX_NEW;
	return message_size;
}

/* 
 * is_vl_message_new: Checks if a VLs payload is new 
 *					  (hasn't been transmitted (txVL) or hasn't been read (rxVL))
 * INPUTS: vl -- vl to check
 * RETURNS: 0 if the message is not new, 1 if the message is new
 */
int is_vl_message_new(VL *vl)
{
	if(!vl)
		return 0;

	if(vl->new_message & (VL_TX_NEW | VL_RX_NEW))
		return 1;
	else
		return 0;
}